/*
 * main.c
 *
 *  Created on: 2013. 4. 30.
 *      Author: Shim
 */
#include "stm32f4_discovery.h"
#include "stm32f4xx_it.h"
#include "main.h"

#include "usbd_cdc_core.h"
#include "usbd_usr.h"
#include "usbd_desc.h"
#include "usbd_cdc_vcp.h"

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

#define TIME_HANDLE_FREQ		100000

// Left Encoder Setting Parameters
#define ENCLA_PIN				GPIO_Pin_15
#define ENCLA_GPIO_PORT			GPIOA
#define ENCLA_GPIO_CLK			RCC_AHB1Periph_GPIOA
#define ENCLA_SOURCE			GPIO_PinSource15
#define ENCLA_AF				GPIO_AF_TIM2

#define ENCLB_PIN				GPIO_Pin_3
#define ENCLB_GPIO_PORT			GPIOB
#define ENCLB_GPIO_CLK			RCC_AHB1Periph_GPIOB
#define ENCLB_SOURCE			GPIO_PinSource3
#define ENCLB_AF				GPIO_AF_TIM2

// Right Encoder Setting Parameters
#define ENCRA_PIN				GPIO_Pin_6
#define ENCRA_GPIO_PORT			GPIOB
#define ENCRA_GPIO_CLK			RCC_AHB1Periph_GPIOB
#define ENCRA_SOURCE			GPIO_PinSource6
#define ENCRA_AF				GPIO_AF_TIM4

#define ENCRB_PIN				GPIO_Pin_7
#define ENCRB_GPIO_PORT			GPIOB
#define ENCRB_GPIO_CLK			RCC_AHB1Periph_GPIOB
#define ENCRB_SOURCE			GPIO_PinSource7
#define ENCRB_AF				GPIO_AF_TIM4

// 사용할 타이머
#define ENCL_TIMER				TIM2
#define ENCL_TIMER_CLK			RCC_APB1Periph_TIM2
#define ENCR_TIMER				TIM4
#define ENCR_TIMER_CLK			RCC_APB1Periph_TIM4

#define LEFT_COUNT()			ENCL_TIMER->CNT
#define RIGHT_COUNT()			ENCR_TIMER->CNT


void InitEncoders(void);
void ResetEncoders(void);
void ReadEncoders(void);
// global variables for encoders
// speeds
volatile int16_t leftCount;
volatile int16_t rightCount;
volatile int16_t fwdCount;
volatile int16_t rotCount;

// angles
volatile int32_t leftTotal;
volatile int32_t rightTotal;
volatile int32_t fwdTotal;
volatile int32_t rotTotal;

// local variables
static volatile int16_t oldLeftEncoder;
static volatile int16_t oldRightEncoder;
static volatile int16_t leftEncoder;
static volatile int16_t rightEncoder;
static volatile int16_t encoderSum;
static volatile int16_t encoderDiff;
static uint8_t isSend = 0;
volatile uint32_t time_var1, time_var2;

__ALIGN_BEGIN USB_OTG_CORE_HANDLE  USB_OTG_dev __ALIGN_END;

#define LEFT_BIT0	(0x00 << 4)
#define LEFT_BIT1	(0x01 << 4)
#define LEFT_BIT2	(0x02 << 4)
#define LEFT_BIT3	(0x03 << 4)

#define RIGHT_BIT0	(0x04 << 4)
#define RIGHT_BIT1	(0x05 << 4)
#define RIGHT_BIT2	(0x06 << 4)
#define RIGHT_BIT3	(0x07 << 4)

// Private function prototypes
void Delay(volatile uint32_t nCount);

/* Private typedef -----------------------------------------------------------*/
GPIO_InitTypeDef GPIOA_InitStructure;
GPIO_InitTypeDef GPIOC_InitStructure;
GPIO_InitTypeDef GPIOD_InitStructure;
EXTI_InitTypeDef EXTI0_InitStructure;
EXTI_InitTypeDef EXTI1_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
USART_InitTypeDef USART2_InitStructure;

char isRotated = 0;

unsigned char prevStateL = 0;
unsigned char currentStateL = 0;
int positionL = 0;

unsigned char prevStateR = 0;
unsigned char currentStateR = 0;
int positionR = 0;
unsigned char toggle = 0;
unsigned char isMeasure = 0;
/**
 * Ansi C "itoa" based on Kernighan & Ritchie's "Ansi C":
 */
void strreverse(uint8_t* begin, uint8_t* end) {
	uint8_t aux;
	while(end>begin)
		aux=*end, *end--=*begin, *begin++=aux;
}

void itoa(int32_t value, uint8_t* str, int32_t base) {
  static uint8_t num[] = "0123456789abcdefghijklmnopqrstuvwxyz";
  uint8_t* wstr=str;
  int32_t sign;
  if (base<2 || base>35){ *wstr='\0'; return; }
  if ((sign=value) < 0) value = -value;
  do *wstr++ = num[value%base]; while(value/=base);
  if(sign<0) *wstr++='-';
  *wstr='\0';
  strreverse(str,wstr-1);
}

int main()
{
	uint8_t sendPacket[10];
	uint16_t lt, rt;
	uint16_t i;


	//SystemCoreClockUpdate();
	/* Enable GPIOA Clock */
	//RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	/* Enable GPIOD Clock */
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
	/* Enable SYSCFG Clock */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
    /* Enable USART2 Clock */
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
	// ---------- SysTick timer -------- //
	// 168,000 Hz
	if (SysTick_Config(SystemCoreClock/TIME_HANDLE_FREQ)) {
		// Capture error
		while (1){};
	}

	/* Configure PA0 pin as input floating */
	/*
	GPIOA_InitStructure.GPIO_Mode = GPIO_Mode_IN;
	GPIOA_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIOA_InitStructure.GPIO_Pin = GPIO_Pin_0;
	GPIOA_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIOA_InitStructure);
*/

	/* Configure PC0 and PC1 Pins as input floating */
/*
	GPIOC_InitStructure.GPIO_Mode = GPIO_Mode_IN;
	GPIOC_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIOC_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;
	GPIOC_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOC, &GPIOC_InitStructure);
*/

	InitEncoders();
	/* Configure PD12~PD15 pins as push-pull output */
	GPIOD_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
	GPIOD_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIOD_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIOD_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIOD_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOD, &GPIOD_InitStructure);

	// ------------- USB -------------- //


	  USBD_Init(&USB_OTG_dev,
	#ifdef USE_USB_OTG_HS
	  USB_OTG_HS_CORE_ID,
	#else
	  USB_OTG_FS_CORE_ID,
	#endif
	  &USR_desc,
	  &USBD_CDC_cb,
	  &USR_cb);



	  VCP_Init();

	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource0);

	/* Connect EXTI Line1 to PC0 and PC1 Pins */
	//SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource1);

	while(1)
	{
		if(isSend == 1)
		{
			lt = leftTotal + 8000;
			rt = rightTotal+ 8000;
			sendPacket[0] = LEFT_BIT0;
			sendPacket[0] += (uint8_t)((lt & 0xf000) >> 12);
			sendPacket[1] = LEFT_BIT1;
			sendPacket[1] += (uint8_t)((lt & 0x0f00) >> 8);
			sendPacket[2] = LEFT_BIT2;
			sendPacket[2] += (uint8_t)((lt & 0x00f0) >> 4);
			sendPacket[3] = LEFT_BIT3;
			sendPacket[3] += (uint8_t)(lt & 0x000f);

			sendPacket[4] = 0xf1;
			sendPacket[5] = RIGHT_BIT0;
			sendPacket[5] += (uint8_t)((rt & 0xf000) >> 12);
			sendPacket[6] = RIGHT_BIT1;
			sendPacket[6] += (uint8_t)((rt & 0x0f00) >> 8);
			sendPacket[7] = RIGHT_BIT2;
			sendPacket[7] += (uint8_t)((rt & 0x00f0) >> 4);
			sendPacket[8] = RIGHT_BIT3;
			sendPacket[8] += (uint8_t)(rt & 0x000f);
			sendPacket[9] = 0xf2;

			for(i = 0 ; i < 10 ; i++) {
				VCP_DataTx(0, sendPacket[i]);
			}

			/*
			itoa(leftTotal, data, 10);

			size = strlen(data);
			for(i = 0 ; i < size ; i++) {

				VCP_DataTx(0, data[i]);
			}
			VCP_DataTx(0, ';');

			itoa(rightTotal, data, 10);
			size = strlen(data);
			for(i = 0 ; i < size ;  i++) {
				VCP_DataTx(0, data[i]);
			}
			VCP_DataTx(0, '\r');
			VCP_DataTx(0, '\n');
			*/
			isSend = 0;
		}
	}
	return 0;
}

/*
 * Called from systick handler
 */

void timing_handler()
{

	static uint16_t  count = 10;
	ReadEncoders();
	if(count-- == 0) {
		if(1)//isMeasure)
		{

			isSend = 1;
		}
		count = 10;
	}
}

void Delay(volatile uint32_t nCount)
{
	time_var1 = nCount;
	while(time_var1){};
}

/*
 * Dummy function to avoid compiler error
 */
void _init() {

}

/*
 * quadrature encoder counter에 사용할 2 개의 타이머를 설정
 * 타이머는 16비트로 사용.
 */

void InitEncoders(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;

	// 각 포트의 클록을 활성화한다.
	RCC_AHB1PeriphClockCmd(ENCLA_GPIO_CLK, ENABLE);
	RCC_AHB1PeriphClockCmd(ENCLB_GPIO_CLK, ENABLE);
	RCC_AHB1PeriphClockCmd(ENCRA_GPIO_CLK, ENABLE);
	RCC_AHB1PeriphClockCmd(ENCRB_GPIO_CLK, ENABLE);

	// 각 핀들을 설정한다.
	GPIO_StructInit(&GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_InitStructure.GPIO_Pin  = ENCLA_PIN;
	GPIO_Init(ENCLA_GPIO_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin  = ENCLB_PIN;
	GPIO_Init(ENCLB_GPIO_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin  = ENCRA_PIN;
	GPIO_Init(ENCRA_GPIO_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin  = ENCRB_PIN;
	GPIO_Init(ENCRB_GPIO_PORT, &GPIO_InitStructure);

	// 각 핀을 alternate function에연결한다.
	GPIO_PinAFConfig(ENCLA_GPIO_PORT, ENCLA_SOURCE, ENCLA_AF);
	GPIO_PinAFConfig(ENCLB_GPIO_PORT, ENCLB_SOURCE, ENCLB_AF);
	GPIO_PinAFConfig(ENCRA_GPIO_PORT, ENCRA_SOURCE, ENCRA_AF);
	GPIO_PinAFConfig(ENCRB_GPIO_PORT, ENCRB_SOURCE, ENCRB_AF);

	// 타이머 주변장치 클록 활성화
	RCC_APB1PeriphClockCmd(ENCL_TIMER_CLK, ENABLE);
	RCC_APB1PeriphClockCmd(ENCR_TIMER_CLK, ENABLE);

	// 인코더 입력으로 설정

	TIM_EncoderInterfaceConfig(ENCL_TIMER, TIM_EncoderMode_TI12,
							   TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	TIM_SetAutoreload(ENCL_TIMER, 0xffff);
	TIM_EncoderInterfaceConfig(ENCR_TIMER, TIM_EncoderMode_TI12,
								   TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
	TIM_SetAutoreload(ENCR_TIMER, 0xffff);

	// 타이머/카운터를 켠다.
	TIM_Cmd(ENCL_TIMER, ENABLE);
	TIM_Cmd(ENCR_TIMER, ENABLE);
	ResetEncoders();
}

void ResetEncoders(void)
{
	__disable_irq();
	oldLeftEncoder = 0;
	oldRightEncoder = 0;
	leftTotal = 0;
	rightTotal = 0;
	fwdTotal = 0;
	rotTotal = 0;
	TIM_SetCounter(ENCL_TIMER, 0);
	TIM_SetCounter(ENCR_TIMER, 0);
	//ReadEncoders();
	__enable_irq();
}

void ReadEncoders(void)
{
	oldLeftEncoder = leftEncoder;
	leftEncoder = TIM_GetCounter(ENCL_TIMER);
	oldRightEncoder = rightEncoder;
	rightEncoder = -TIM_GetCounter(ENCR_TIMER);
	leftCount = leftEncoder - oldLeftEncoder;
	rightCount = rightEncoder-oldRightEncoder;
	fwdCount = leftCount + rightCount;
	rotCount = -leftCount - rightCount;

	fwdTotal += fwdCount;
	rotTotal += rotCount;

	leftTotal += leftCount;
	rightTotal += rightCount;
}
